import os
import threading
import time

from PyQt5.QtGui import QPixmap, QImage
from PyQt5.QtCore import Qt, QThread, QMutex, QWaitCondition
from PyQt5.QtWidgets import QApplication, QMessageBox
from PyQt5.QtWidgets import QFileDialog
from PyQt5.QtCore import QTimer, QDateTime
import json
from grab_win import GrabWindow ,QSSLoader
from PyQt5.QtCore import QUrl
from PyQt5.QtGui import QDesktopServices
from camera import Camera
from threading import *
from robot_arm import RobotArm
from pv_agv import AGV
from task import Task
from PyQt5.QtCore import QObject, pyqtSignal
from techlego_sdk import binary_protocol, scan3d_client,collect_parameters
from scanner import Scanner


class Controller(QObject):
    def __init__(self, win: GrabWindow):
        super().__init__()
        self.win = win
        self.cam = None
        self.arm = None
        self.model = None
        self.color_image = None
        self.depth_image = None
        self.timer = QTimer(win)
        self.timer.timeout.connect(self.onTimer)
        self.image_update_thread = None
        self.light = False
        self.thread_hand_eye_data_record = None
        self.task = None
        self.point_temp_file_path = None
        self.point_temp_data = {}
        self.json_point_temp_file = None
        self.scanner = None
        self.point_delta = None
        self.task = None
        self.taskConfig = None
        self.arm_default_pose_file = "./config/arm_default_pose.json"
        with open(self.arm_default_pose_file, "r+") as f:
            self.dict_arm_default_and_obs_pose = json.load(f)

        # self.timer.start(1000)  # 设置定时器每1000毫秒（1秒）触发一次
        # task
        self.win.ui.pushButtonTaskRun.clicked.connect(self.onButtonRunTask)
        self.win.ui.pushButtonTaskLoad.clicked.connect(self.onButtonLoadTask)

        self.win.ui.pushButtonTaskPause.clicked.connect(self.onButtonPauseTask)
        self.win.ui.pushButtonTaskResume.clicked.connect(self.onButtonResumeTask)
        self.win.ui.pushButtonTaskCancel.clicked.connect(self.onButtonCancelTask)

        # camera
        self.win.ui.pushButtonConnectCam.clicked.connect(self.onButtonCameraSetting)
        self.win.ui.pushButtonDisconnectCam.clicked.connect(self.onButtonCameraClose)

        # ARM
        self.win.ui.pushButtonConnectARM.clicked.connect(self.onButtonConnectARM)
        self.win.ui.pushButtonDisconnectARM.clicked.connect(self.onButtonDisconnectARM)

        # AGV
        self.win.ui.pushButtonConnectAGV.clicked.connect(self.onButtonConnectAGV)
        self.win.ui.pushButtonDisconnectAGV.clicked.connect(self.onButtonDisconnectAGV)
        self.win.ui.pushButtonAGVSetting.clicked.connect(self.onButtonAGVSetting)


        # handeye
        self.win.ui.pushButtonHandEye.clicked.connect(self.onButtonHandEye)

        # detect calib
        self.win.ui.pushButtonDetectCalib.clicked.connect(self.onButtonDetectCalib)

        # light
        self.win.ui.pushButtonLight.clicked.connect(self.onButtonLight)

        # Point Temp
        self.win.ui.pushButtonLoadPointTemp.clicked.connect(self.onButtonLoadPointTemp)
        self.win.ui.pushButtonCreatePointTemp.clicked.connect(self.onButtonCreatePointTemp)
        self.win.ui.pushButtonSavePoint.clicked.connect(self.onButtonSavePoint)
        self.win.ui.pushButtonSavePointTemp.clicked.connect(self.onButtonSavePointTemp)

        #scanner
        self.win.ui.pushButtonOpenScanner.clicked.connect(self.onButtonOpenScanner)
        self.win.ui.pushButtonSelectScanTemp.clicked.connect(self.onButtonSelectScannerTemp)
        self.win.ui.pushButtonScan.clicked.connect(self.onButtonScanFromeCurrentPose)

        #arm default and obs pose
        self.win.ui.pushButtonSaveArmDefaultPose.clicked.connect(self.onButtonSaveArmDefaultPose)
        self.win.ui.pushButtonSaveArmObsCalibPose.clicked.connect(self.onButtonSaveArmObsCalibPose)
        self.win.ui.pushButtonMoveObsPose.clicked.connect(self.onButtonMoveObsPose)
        self.win.ui.pushButtonMoveDefaultPose.clicked.connect(self.onButtonMoveDefaultPose)
        self.win.ui.pushButtonArmChangeAutoPowerOn.clicked.connect(self.onButtonArmChangeAutoPowerOn)
        self.win.ui.pushButtonAGVMoveToPoint.clicked.connect(self.onButtonAGVMoveToPoint)
        self.win.ui.pushButtonSaveScanPose.clicked.connect(self.onButtonSaveArmScanPose)

    def onButtonAGVMoveToPoint(self):
        station_name = self.win.ui.lineEditAGVStation.text()
        staton_list = []
        staton_list.append(station_name)
        self.agv.move_to_station_list(staton_list)

    def onButtonArmChangeAutoPowerOn(self):
        self.arm.change_auto_power_on()

    def onButtonMoveDefaultPose(self):
        pose = self.dict_arm_default_and_obs_pose['arm_default_pose']
        self.arm.movel_rokae(pose)

    def onButtonMoveObsPose(self):
        pose = self.dict_arm_default_and_obs_pose['arm_obs_pose']
        self.arm.movel_rokae(pose)

    def onButtonSaveArmObsCalibPose(self):
        pose = self.arm.get_end_rokae_type()
        data = {"arm_obs_pose":pose}
        with open(self.arm_default_pose_file, "r+") as f:
            self.dict_arm_default_and_obs_pose = json.load(f)
            self.dict_arm_default_and_obs_pose.update(data)
        time.sleep(0.5)
        with open(self.arm_default_pose_file, "w+") as f:
            json.dump(self.dict_arm_default_and_obs_pose, f)

    def onButtonSaveArmDefaultPose(self):
        pose = self.arm.get_end_rokae_type()
        data = {"arm_default_pose": pose}
        with open(self.arm_default_pose_file, "r+") as f:
            self.dict_arm_default_and_obs_pose = json.load(f)
            self.dict_arm_default_and_obs_pose.update(data)
        time.sleep(0.5)
        with open(self.arm_default_pose_file, "w+") as f:
            json.dump(self.dict_arm_default_and_obs_pose, f)

    def onButtonSaveArmScanPose(self):
        pose = self.arm.get_end_rokae_type()
        data = {"arm_scan_pose": pose}
        with open(self.arm_default_pose_file, "r+") as f:
            self.dict_arm_default_and_obs_pose = json.load(f)
            self.dict_arm_default_and_obs_pose.update(data)
        time.sleep(0.5)
        with open(self.arm_default_pose_file, "w+") as f:
            json.dump(self.dict_arm_default_and_obs_pose, f)

    def onButtonScanFromeCurrentPose(self):
        current_pose = self.arm.get_end_rokae_type()
        delta_list = self.point_delta["delta_list"]
        aim_pose_list = []
        for delta in delta_list:
            aim_pose = current_pose[:]
            aim_pose = [current_pose[i] + delta[i] for i in range(0,len(current_pose))]
            aim_pose_list.append(aim_pose)

        # num = self.scanner.get_total_scan_groups()
        # if num !=len(aim_pose_list):
        #     QMessageBox.warning(self.win, '失败', '打开工程失败', QMessageBox.Yes | QMessageBox.No,
        #                         QMessageBox.Yes)
        for i in range(len(aim_pose_list)):
            # move 移动
                self.arm.movel_rokae(aim_pose_list[i])
                # self.scanner.start_scan_by_index(i)


    def onButtonSavePointTemp(self):
        if self.point_temp_data:
            init_pose = self.point_temp_data[0].value()
            index = 0
            list_delta = []
            for item_value in self.point_temp_data.values():
                if index == 0:
                   init_delta = [0,0,0,0,0,0]
                   list_delta.append(init_delta)
                   continue
                delta = [item_value[i] - init_pose[i]  for i in range(0, len(item_value))]
                delta[3:] = [0,0,0]
                list_delta.append(delta)
            json_dict = {"delta_list": list_delta}
            with open(self.point_temp_file_path, 'w+') as f:
                json.dump(json_dict, f)
                self.point_temp_data = {}


    def onButtonOpenScanner(self):
        # soft_path =  "\"D:\\Program Files\\Techlego\\Techlego\\Techlego.exe\""
        # t = threading.Thread(target=os.system ,args=(soft_path,))
        # t.daemon = True
        # t.start()
        # # os.system(soft_path)
        # time.sleep(3)
        #
        # protocol = binary_protocol("localhost", 5252)
        # #通过协议创建客户端
        # client = scan3d_client(protocol)
        # #设置快速扫描
        # obj = collect_parameters()
        # obj.set_fast_scan(True,True)
        # #设置
        # client.collect_params_window_set(obj)
        # #打开相机
        # client.open_camera(0)
        #
        # #根据项目文件打开项目，项目文件不存在会失败
        # #开始扫描
        # num = client.get_total_scan_groups()
        self.scanner = Scanner()

    def onButtonSelectScanTemp(self):
        if self.scanner is None:
            # QMessageBox.information(self.win, "失败", "扫描仪没有打开", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '扫描仪没有打开', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return

        file_path, _ = QFileDialog.getOpenFileName(self.win, "选择文件", "C:/", "vtop文件(*.vtop)")
        self.win.ui.textBrowserScanLog.append("打开工程模板")
        self.win.ui.textBrowserScanLog.append("当前工程模板: " + file_path)
        if file_path:
            is_open = self.scanner.open_project(file_path)
            if is_open == False:
                # QMessageBox.information(self.win, "失败", "打开工程失败", QMessageBox.Yes)
                QMessageBox.warning(self.win, '失败', '打开工程失败', QMessageBox.Yes | QMessageBox.No,
                                    QMessageBox.Yes)
                return

    def onButtonLoadPointTemp(self):
        file_path, _ = QFileDialog.getOpenFileName(self.win, "选择文件", "C:/", "json文件(*.json)")

        self.win.ui.textBrowserScanLog.append("创建点位模板成功")
        self.win.ui.textBrowserScanLog.append("当前文件: " + file_path)
        if file_path:
            self.point_temp_file_path = file_path
            try:
                with open(self.point_temp_file_path, 'r+') as f:
                    self.json_point_temp_file = f
                    self.point_delta = json.load(f)

                    print("选择的目录路径：", self.point_delta)
            except json.decoder.JSONDecodeError:
                QMessageBox.warning(self.win, '失败', '载入数据格式有问题或者为空文件', QMessageBox.Yes | QMessageBox.No,
                                    QMessageBox.Yes)
                print("JSON 文件为空或包含无效数据")
        pass

    def onButtonCreatePointTemp(self):

        filepath, _ = QFileDialog.getSaveFileName(self.win, "文件保存", "/", '文本文件 (*.json)')
        self.win.ui.textBrowserScanLog.append("创建点位模板成功")
        self.win.ui.textBrowserScanLog.append("当前文件: "+filepath)
        if filepath:
            self.point_temp_file_path = filepath
            with open(self.point_temp_file_path, 'w+') as f:
                pass


    def onButtonSavePoint(self):
        if self.arm is None:
            QMessageBox.warning(self.win, '失败', '机械臂未连接,无法获取点位', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        point_name = self.win.ui.lineEditPointName.text()
        arm_pose_rokae_type = self.arm.get_end_rokae_type()
        data = {point_name: arm_pose_rokae_type}
        self.point_temp_data.update(data)
        self.win.ui.textBrowserScanLog.append(str(data))
        # with open(self.point_temp_file_path, 'w+') as f:
            #     json.dump(self.json_point_temp_data,f)

    def onButtonSavePointTemp(self):
        if self.point_temp_data:
            init_pose = list(self.point_temp_data.items())[0][1]
            init = 0
            list_delta = []
            for item_value in self.point_temp_data.values():
                if init == 0:
                    init_delta = [0,0,0,0,0,0]
                    list_delta.append(init_delta)
                    init = -1
                    continue
                delta = [item_value[i] - init_pose[i]  for i in range(0, len(item_value))]
                delta[3:] = [0,0,0]
                list_delta.append(delta)
            json_dict = {"delta_list": list_delta}
            with open(self.point_temp_file_path, 'w+') as f:
                json.dump(json_dict, f)
                self.win.ui.textBrowserScanLog.append("保存数据:"+str(json_dict))
                self.win.ui.textBrowserScanLog.append("到文件" + self.point_temp_file_path)
                self.point_temp_data = {}

    def onButtonLight(self):
        if self.arm is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '机械臂未连接,光源是通过机械臂IO的DO_0控制的', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        ec = {}

        if self.light == False:
            self.arm.robot.setDO(0,0,True, ec)
            self.light = True
        else:
            self.arm.robot.setDO(0,0,False, ec)
            self.light = False

    def onButtonDetectCalib(self):
        if self.cam is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '相机未连接', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        if self.arm is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '机械臂未连接', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        # 获得相机位置
        ec = {}
        self.arm.robot.setDO(0, 0, True, ec)
        re, cam_t_calib = self.cam.gen_cam_t_calib(type='halcon')
        if re == False:
            QMessageBox.warning(self.win, '失败', '无法检测到标定板:检查(1)清晰度，(2)光源状态,(3)标定板类型', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        # 获得机械臂位置
        end_t_base_rokae_tpye = self.arm.get_end_rokae_type()
        filepath, _ = QFileDialog.getSaveFileName(self.win, "文件保存", "/", '文本文件 (*.json)')
        import json
        data = {
            'cam_t_calib_halcon_type': cam_t_calib,
            'end_t_base_rokae_type':end_t_base_rokae_tpye
        }
        if filepath:
            with open(filepath, 'w') as file:
                json.dump(data, file)
                print(f"文件已保存到: {filepath}")

    def onButtonHandEye(self):
        if self.cam is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '相机未连接', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        if self.arm is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '机械臂未连接', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return

        from record_hand_eye_auto import handeye_data_record

        self.thread_hand_eye_data_record = Thread(target=handeye_data_record, kwargs={"ca": self.cam, "arm": self.arm}, daemon=True)
        self.thread_hand_eye_data_record.start()

    def onButtonConnectAGV(self):
        IP = self.win.ui.lineEditAGVIP.text()
        self.agv = AGV(IP, "admin", "admin")
        self.win.ui.labelAGVStateLight.setStyleSheet \
            ('''QLabel{background-color: rgb(0, 234, 0);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')
        pass

    def onButtonDisconnectAGV(self):
        self.win.ui.labelAGVStateLight.setStyleSheet \
            ('''QLabel{background-color: rgb(58, 111, 50);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')
        pass

    def onButtonConnectARM(self):
        IP = self.win.ui.lineEditARMIP.text()
        # self.cam = Camera()

        self.arm = RobotArm(IP)
        ec = {}
        # self.arm.robot.setDO(1,0,True,ec)
        # self.arm.robot.setDO(2,0,True,ec)
        self.win.ui.labelARMStateLight.setStyleSheet \
            ('''QLabel{background-color: rgb(0, 234, 0);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')

        pass

    def onButtonDisconnectARM(self):
        self.win.ui.labelARMStateLight.setStyleSheet \
            ('''QLabel{background-color: rgb(58, 111, 50);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')
        pass

    def onButtonCameraSetting(self):
        cam_name = self.win.ui.lineEditCameraInfo.text()
        self.cam = Camera(camera_name=cam_name)
        self.timer.start(100)

        self.image_update_thread  = Thread(target=self.cam.update_image_loop, daemon=True)
        self.image_update_thread.start()
        self.win.ui.labelCameraStateLight.setStyleSheet\
            ('''QLabel{background-color: rgb(0, 234, 0);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')

    def onButtonCameraClose(self):
        self.cam.close()
        self.win.ui.labelCameraStateLight.setStyleSheet \
            ('''QLabel{background-color: rgb(58, 111, 50);border-radius: 40px; border: 3px groove gray;border-style: outset;}''')

    def onButtonAGVSetting(self):
        # self.win.ui.lineEditAGVIP.get
        QDesktopServices.openUrl(QUrl("http://192.168.100.100:8080"))

    def onButtonPauseTask(self):
        if self.task is not None:
            self.task.pause()
        self.win.ui.textBrowserTaskInfo.append("*任务暂停执行")

    def onButtonResumeTask(self):
        if self.task is not None:
            self.task.resume()
        self.win.ui.textBrowserTaskInfo.append("*任务恢复执行")

    def onButtonCancelTask(self):
        if self.task is not None:
            self.task.cancel()
        self.win.ui.textBrowserTaskInfo.append("*任务取消执行")

    def onButtonToolClose(self):
        self.arm.setEndClose()

    def onButtonToolOpen(self):
        self.arm.setEndOpen()

    def onTimer(self):
        self.color_image = self.cam.read_opencv_image()
        self.update_image(self.color_image)


    def convert_cv_qt(self, img):
        """将 OpenCV 图像转换为 QImage"""
        if len(img.shape) == 2:  # 灰度图
            height, width = img.shape
            bytes_per_line = width
            qimg = QImage(img.data, width, height, bytes_per_line, QImage.Format_Grayscale8)
        else:  # 彩色图
            height, width, channel = img.shape
            bytes_per_line = 3 * width
            qimg = QImage(img.data, width, height, bytes_per_line, QImage.Format_RGB888).rgbSwapped()
        return qimg

    def update_image(self,image):
        """更新 QLabel 上显示的图像"""
        qimg = self.convert_cv_qt(image)
        pixmap = QPixmap.fromImage(qimg)
        self.win.ui.ImageQlabel.setPixmap(pixmap)
        self.win.ui.ImageQlabel.setAlignment(Qt.AlignCenter)

    def onButtonLoadTask(self):
        if self.cam is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '相机未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        if self.arm is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', '机械臂未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        if self.agv is None:
            # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
            QMessageBox.warning(self.win, '失败', 'AGV未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return

        dialog = QFileDialog()
        dialog.setFileMode(QFileDialog.ExistingFiles)
        dialog.setOption(QFileDialog.DontUseNativeDialog, True)
        dialog.setAcceptMode(QFileDialog.AcceptOpen)
        # file_path, _ = dialog.getOpenFileName(None, "选择文件", "C:/", "json文件(*.json)")
        self.Path = os.getcwd()
        dialog.exec_()
        openfile_name = dialog.selectedFiles()
        print(openfile_name[0])
        if len(openfile_name) != 0:
            self.task = Task()
            self.task.set_task_json(openfile_name[0])
            self.task.set_device(self.arm, self.cam, self.agv, self.win.ui)
            self.task.set_scanner(self.scanner)
            self.win.ui.textBrowserTaskInfo.append("*任务加载成功")
        else:
            QMessageBox.warning(self.win, '失败', '未选择任务', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)


    def onButtonRunTask(self):
        if  self.task is None:
            QMessageBox.warning(self.win, '失败', '任务未配置，请先加载配置文件', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
            return
        self.task.run_thread()
        self.win.ui.textBrowserTaskInfo.append("*任务运行成功")

    def onButtonEditTask(self):
        print("d")
        # file_path, _ = QFileDialog.getOpenFileName(self, "选择文件", "C:/", "json文件(*.json)")
        #
        # if file_path:
        #     print("选择的目录路径：", file_path)

    def onButtonSelectScannerTemp(self):
        # if self.cam is None:
        #     # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
        #     QMessageBox.warning(self.win, '失败', '相机未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
        #     return
        # if self.arm is None:
        #     # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
        #     QMessageBox.warning(self.win, '失败', '机械臂未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
        #     return
        # if self.agv is None:
        #     # QMessageBox.information(self.win, "失败", "这是一个信息提示框", QMessageBox.Yes)
        #     QMessageBox.warning(self.win, '失败', 'AGV未连接:任务需要该设备才能运行', QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes)
        #     return

        dialog = QFileDialog()
        dialog.setFileMode(QFileDialog.ExistingFiles)
        dialog.setOption(QFileDialog.DontUseNativeDialog, True)
        dialog.setAcceptMode(QFileDialog.AcceptOpen)
        dialog.exec_()
        openfile_name = []
        openfile_name = dialog.selectedFiles()
        print(openfile_name[0])
        if len(openfile_name) != 0:
            self.win.ui.textBrowserScanLog.append("扫描模板加载成功")
